//Mostly copied from Rosetta Code ( http://rosettacode.org/wiki/QR_decomposition#C )
//Changes: threads, memory allocation, time measure - the algorithm is intact

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <sys/time.h>
#include "timetools.h"

extern "C" {

int num_mats, N;
double ***matx;


typedef struct {
	int m, n;
	double ** v;
} mat_t, *mat;
 

mat matrix_new(int m, int n)
{
	mat x = (mat) malloc(sizeof(mat_t));
	x->v = (double**) malloc(sizeof(double) * m);
	x->v[0] = (double*) calloc(sizeof(double), m * n);
	for (int i = 0; i < m; i++)
		x->v[i] = x->v[0] + n * i;
	x->m = m;
	x->n = n;
	return x;
}

void matrix_delete(mat m)
{
	free(m->v[0]);
	free(m->v);
	free(m);
}


void matrix_transpose(mat m)
{
	for (int i = 0; i < m->m; i++) {
		for (int j = 0; j < i; j++) {
			double t = m->v[i][j];
			m->v[i][j] = m->v[j][i];
			m->v[j][i] = t;
		}
	}
}


mat matrix_copy(double **a, int m, int n)
{
	mat x = matrix_new(m, n);
	for (int i = 0; i < m; i++)
		for (int j = 0; j < n; j++)
			x->v[i][j] = a[i][j];
	return x;
}


mat matrix_mul(mat x, mat y)
{
	if (x->n != y->m) return 0;
	mat r = matrix_new(x->m, y->n);
	for (int i = 0; i < x->m; i++)
		for (int j = 0; j < y->n; j++)
			for (int k = 0; k < x->n; k++)
				r->v[i][j] += x->v[i][k] * y->v[k][j];
	return r;
}


mat matrix_minor(mat x, int d)
{
	mat m = matrix_new(x->m, x->n);
	for (int i = 0; i < d; i++)
		m->v[i][i] = 1;
	for (int i = d; i < x->m; i++)
		for (int j = d; j < x->n; j++)
			m->v[i][j] = x->v[i][j];
	return m;
}
 

double *vmadd(double a[], double b[], double s, double c[], int n)
{
	for (int i = 0; i < n; i++)
		c[i] = a[i] + s * b[i];
	return c;
}
 

mat vmul(double v[], int n)
{
	mat x = matrix_new(n, n);
	for (int i = 0; i < n; i++)
		for (int j = 0; j < n; j++)
			x->v[i][j] = -2 *  v[i] * v[j];
	for (int i = 0; i < n; i++)
		x->v[i][i] += 1;
 
	return x;
}
 

double vnorm(double x[], int n)
{
	double sum = 0;
	for (int i = 0; i < n; i++) sum += x[i] * x[i];
	return sqrt(sum);
}
 

double* vdiv(double x[], double d, double y[], int n)
{
	for (int i = 0; i < n; i++) y[i] = x[i] / d;
	return y;
}
 

double* mcol(mat m, double *v, int c)
{
	for (int i = 0; i < m->m; i++)
		v[i] = m->v[i][c];
	return v;
}


void
__attribute__((noinline))
householder(mat m, mat *R, mat *Q)
{
	mat q[m->m];
	mat z = m, z1;
	for (int k = 0; k < m->n && k < m->m - 1; k++) {
		double e[m->m], x[m->m], a;
		z1 = matrix_minor(z, k);
		if (z != m) matrix_delete(z);
		z = z1;
 
		mcol(z, x, k);
		a = vnorm(x, m->m);
		if (m->v[k][k] > 0) a = -a;
 
		for (int i = 0; i < m->m; i++)
			e[i] = (i == k) ? 1 : 0;
 
		vmadd(x, e, a, e, m->m);
		vdiv(e, vnorm(e, m->m), e, m->m);
		q[k] = vmul(e, m->m);
		z1 = matrix_mul(q[k], z);
		if (z != m) matrix_delete(z);
		z = z1;
	}
	matrix_delete(z);
	*Q = q[0];
	*R = matrix_mul(q[0], m);
	for (int i = 1; i < m->n && i < m->m - 1; i++) {
		z1 = matrix_mul(q[i], *Q);
		if (i > 1) matrix_delete(*Q);
		*Q = z1;
		matrix_delete(q[i]);
	}
	matrix_delete(q[0]);
	z = matrix_mul(*Q, m);
	matrix_delete(*R);
	*R = z;
	matrix_transpose(*Q);
}


void* worker_thread() {
	int i = num_mats;
	
	while( i > 0 ) {
		mat x = matrix_copy(matx[i-1], N, N);
		mat R, Q;
		
		householder(x, &R, &Q);
		 
		//matrix_delete(x);
		matrix_delete(R);
		matrix_delete(Q);
		i--;
	}
	return NULL;
}


int main(int argc, char *argv[]) {


	if (argc != 3) {
		printf("ERROR...Usage:%s num_matrices matrix_dimension\n",argv[0]);
		return -1;
	}

	num_mats = atoi(argv[1]);
	N = atoi(argv[2]);
	
	int j,k,w;

	matx = (double***) malloc(num_mats*sizeof(double**));

    for (j=0;j<num_mats;++j) {
        matx[j] = (double**) malloc(N*sizeof(double*));

        for (k=0;k<N;++k) {
            matx[j][k] = (double*) malloc(N*sizeof(double));
            for (w=0;w<N;++w)
                matx[j][k][w] = 1.0;
        }
    }

#ifdef ENABLE_OUTPUT
	struct timespec startTime,endTime;
	readTime(&startTime);
#endif
	    worker_thread();
#ifdef ENABLE_OUTPUT
    readTime(&endTime);

    const double elapsedTime = timeDiff(startTime, endTime);
    printf("SIZE %d\nTIME %f\n", num_mats, elapsedTime);
#endif
	
    for (j=0;j<num_mats;++j) {
        for (k=0;k<N;++k) { free(matx[j][k]); }
        free(matx[j]);
    }
    free(matx);
	
	return 0;
}

}
